#include <mc_path_planner/graph_planner/jps.h>


struct CompareNodes {
    bool operator()(const Node2DPtr& lhs, const Node2DPtr& rhs) const {
        return lhs->getF() > rhs->getF() || (lhs->getF() == rhs->getF() && lhs->getH() > rhs->getH());
    //     return lhs->getF() > rhs->getF() || 
    //         ((lhs->getF() >= rhs->getF() - 0.000001) 
    //             && (lhs->getF() <= rhs->getF() + 0.000001)
    //             && lhs->getH() > rhs->getH());
    }
};


JPSPlanner::JPSPlanner(boost::shared_ptr<MapBase> map_handler_ptr):
    PlannerBase(map_handler_ptr){ 
    debugMapPub_ = nh_.advertise<nav_msgs::OccupancyGrid>("/jps_expend_map", 1, true);
    GLOG_INFO <<  "[JPSPlanner] init......";

    debug_ = true;
}

JPSPlanner::~JPSPlanner(){

}

// 路径规划
mc_status JPSPlanner::pathGenerate(Point2i start, Point2i goal, vector<Point2i>& points){
    mc_status status;
    points.clear();
    int width = mapHandlerPtr_->getWidth();
    int height = mapHandlerPtr_->getHeight();

    if(debug_){
        nav_msgs::OccupancyGrid::Ptr originMapPtr = mapHandlerPtr_->getOccupancyGrid();
        debugGrid_.header = originMapPtr->header;
        debugGrid_.info = originMapPtr->info;
        debugGrid_.data.resize(width * height);
        std::fill(debugGrid_.data.begin(), debugGrid_.data.end(), 0); 
    }

    nodeStart_ = Node2D(start.x, start.y, 0, 0);
    nodeGoal_ = Node2D(goal.x, goal.y, 0, 0);
    nodeStart_.setIdx(width);
    nodeGoal_.setIdx(width);
    nodeStart_.updateH(nodeGoal_);

    std::priority_queue<Node2DPtr, std::vector<Node2DPtr>, CompareNodes> openList;
    std::unordered_map<int, float> closedList;  //存储G值
    openList.push(std::make_shared<Node2D>(nodeStart_));
    
    int count = 0;
    while(!ros::isShuttingDown() && !openList.empty()){
        // 获取优先级最高节点
        auto nodePred = openList.top();
        openList.pop();
        int idxPred = nodePred->getIdx();

        if(debug_){
            int idx = (height - nodePred->getY()) * width + nodePred->getX();
            if(idx >= 0 && idx < debugGrid_.data.size())
                debugGrid_.data[idx] = max(10, int(debugGrid_.data[idx]));
        }

        count ++;
        // if(count > 10) break;
        // GLOG_INFO << "idxPred: " << idxPred 
        //     << ", x: " << nodePred->getX() 
        //     << ", y: " << nodePred->getY()
        //     << ", size: " << openList.size();

        if(closedList.find(idxPred) != closedList.end()){
            continue;
        }
        closedList[idxPred] = nodePred->getG();

        // 发现目标节点
        if(*nodePred == nodeGoal_){
            GLOG_INFO << "find goal.";
            while(nodePred->getPred() != nullptr){
                points.emplace_back(Point2i(nodePred->getX(), nodePred->getY()));
                nodePred = nodePred->getPred();
            } 
            // 添加起点
            points.emplace_back(Point2i(nodePred->getX(), nodePred->getY()));
            std::reverse(points.begin(), points.end());   
            GLOG_INFO << "points size: " << points.size();
            break;
        }

        // 遍历邻域节点
        for(int i = 0; i < Node2D::dir; i++) {
            auto nodeSucc = JumpPoint(nodePred, Node2D::dx[i], Node2D::dy[i]);
            if(nodeSucc == nullptr) {
                continue;
            }
            int idxSucc = nodeSucc->getIdx();

            if(debug_){
                int idx = (height - nodeSucc->getY()) * width + nodeSucc->getX();
                if(idx >= 0 && idx < debugGrid_.data.size())
                    debugGrid_.data[idx] = max(100, int(debugGrid_.data[idx]));
            }

            // 障碍物检测 是否在闭列表
            if(closedList.find(idxSucc) != closedList.end()){
                continue;
            }
            // GLOG_INFO << "--> i: " << i 
            //     << ", idxSucc: " << idxSucc 
            //     << ", x: " << nodeSucc->getX() 
            //     << ", y: " << nodeSucc->getY();
            // nodeSucc->setPred(nodePred);
            openList.push(nodeSucc);   

            // // G值比较
            // nodeSucc->updateG();
            // float newG = nodeSucc->getG();
            // if(closedList.find(idxSucc) == closedList.end() || newG < closedList[idxSucc]) {
            //     nodeSucc->updateH(nodeGoal_);
            //     // GLOG_INFO << "--> i: " << i 
            //     //     << ", idxSucc: " << idxSucc 
            //     //     << ", x: " << nodeSucc->getX() 
            //     //     << ", y: " << nodeSucc->getY();
            //     openList.push(nodeSucc);
            // }             
        }
    }

    cout << "openList size: " << openList.size() 
        << ", closedList size: " << closedList.size()
        << std::endl;

    if(debug_){
        debugMapPub_.publish(debugGrid_);
    }
    return status;
}


// 检测跳点
Node2DPtr JPSPlanner::JumpPoint(Node2DPtr node, int dir_x, int dir_y){
    int x = node->getX();
    int y = node->getY();
    // cout << "[JumpPoint 001] --> x: " << x << ", y: " << y << ", idx: " << node->getIdx() 
    //     << ", dir_x: " << dir_x << ", dir_y: " << dir_y << std::endl;
    int width = mapHandlerPtr_->getWidth();
    int height = mapHandlerPtr_->getHeight();
    int newX = x + dir_x;
    int newY = y + dir_y;
    auto nodeSucc = std::make_shared<Node2D>(newX, newY, node->getG(), 0); 
    nodeSucc->setPred(node);
    nodeSucc->setIdx(width); 
    nodeSucc->updateG();
    nodeSucc->updateH(nodeGoal_);

    if(debug_){
        int idx = (height - nodeSucc->getY()) * width + nodeSucc->getX();
        if(idx >= 0 && idx < debugGrid_.data.size())
            debugGrid_.data[idx] = max(10, int(debugGrid_.data[idx]));
    }

    // cout << "[JumpPoint 002] --> x: " << x << ", y: " << y << ", idx: " << nodeSucc->getIdx() << std::endl;

    // 障碍物检测
    if(!mapHandlerPtr_->ValueIsValid(Point2i(x, y))){
        return nullptr;
    }

    // 终点检测
    if(*nodeSucc == nodeGoal_){
        return nodeSucc;
    }

    if(dir_x && dir_y){     // 对角线检测
    // cout << "[JumpPoint 003] ......" << std::endl;
        // horizontal 水平分量检测
        // vertical 垂直分量检测
        auto horizontal = JumpPoint(nodeSucc, dir_x, 0);
        auto vertical = JumpPoint(nodeSucc, 0, dir_y);
        if(horizontal || vertical){
            return horizontal ? horizontal : vertical;
        }
        // if(JumpPoint(nodeSucc, dir_x, 0) || JumpPoint(nodeSucc, 0, dir_y)){
        //     return nodeSucc;
        // }
    }
    else{   // 强制邻居检测
        // cout << "[JumpPoint 004] ......" << std::endl;
        if(detectForceNeighbor(nodeSucc, dir_x, dir_y)){
            return nodeSucc;
        }
    }

    // cout << "[JumpPoint 005] ......" << std::endl;
    return JumpPoint(nodeSucc, dir_x, dir_y);    
}


// 检测强制邻居
bool JPSPlanner::detectForceNeighbor(Node2DPtr node, int dir_x, int dir_y){
    int x = node->getX();
    int y = node->getY();

    // 水平检测 horizontal
    if(dir_x && !dir_y){
        if( !mapHandlerPtr_->ValueIsValid(Point2i(x, y+1)) && 
             mapHandlerPtr_->ValueIsValid(Point2i(x + dir_x, y+1))){
            return true;
        }

        if( !mapHandlerPtr_->ValueIsValid(Point2i(x, y-1)) && 
             mapHandlerPtr_->ValueIsValid(Point2i(x + dir_x, y-1))){
            return true;
        }
    }

    // 垂直检测 vertical
    if(!dir_x && dir_y){
        if( !mapHandlerPtr_->ValueIsValid(Point2i(x+1, y)) && 
             mapHandlerPtr_->ValueIsValid(Point2i(x + 1, y + dir_y))){
            return true;
        }

        if( !mapHandlerPtr_->ValueIsValid(Point2i(x-1, y)) && 
             mapHandlerPtr_->ValueIsValid(Point2i(x - 1, y + dir_y))){
            return true;
        }
    }

    // 斜对角检测 diagonal
    if(dir_x && dir_y){
        if( !mapHandlerPtr_->ValueIsValid(Point2i(x - dir_x, y)) && 
             mapHandlerPtr_->ValueIsValid(Point2i(x - dir_x, y + dir_y))){
            return true;
        }

        if( !mapHandlerPtr_->ValueIsValid(Point2i(x, y - dir_y)) && 
             mapHandlerPtr_->ValueIsValid(Point2i(x + dir_x, y - dir_y))){
            return true;
        }
    }


    return false;
}

